﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows.Forms;

namespace ModbusRTU_CRC
{
   public  class ClampHelper: IClampMotor

    {
        private SerialPort m_sp;
        private AutoResetEvent are_clamp = new AutoResetEvent(true);
        private int timeout_error = 3000;
        /// <summary>
        /// 是否阻塞
        /// </summary>
        public bool Isblock = true;
        public ClampHelper(SerialPort sp,int timeout=3000)
        {
             m_sp = sp;
            timeout_error = timeout;
        }
        
        /// <summary>
        /// 判断是否串口是否打开
        /// </summary>
        /// <returns></returns>
        public bool serailPortJudge()
        {
            if (m_sp==null || (!m_sp.IsOpen))
            {
                return false;
            }
            return true;
        }
        #region 控制
      
        /// <summary>
        /// 初始化    
        /// </summary>
        /// <returns></returns>
        public bool Inital()
        {
            try
            {
                string cmdStr = "010600000001";//1，单方向校准，（校准方向可以设置）
                var src_crc = GetCRC16(cmdStr);
                var str=writeByte(src_crc);
                if(str.IndexOf(cmdStr) ==0)
                {
                    Stopwatch sw = new Stopwatch();
                    sw.Restart();
                    while (GetInital_Status() != 5)
                    {
                        if (sw.ElapsedMilliseconds > timeout_error)
                            return false;
                    }
                    return true;
                }else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                return false;
            }
        }

        /// <summary>
        /// 夹紧位置
        /// </summary>
        /// <param name="pos">单位 mm，取值范围：0~20</param>
        /// <returns></returns>
        public bool Clamp_Pos(float pos = 0)
        {
            try
            {
                string cmdStr = "01100002000204" + FloatToByteString(pos);
                var src_crc = GetCRC16(cmdStr);
                var str = writeByte(src_crc);
                if (str.IndexOf("011000020002") == 0)
                {
                    if (Isblock)//是否阻塞等待到位
                    {
                        Stopwatch sw = new Stopwatch();
                        sw.Restart();
                        while (GetClamp_Status()!=2|| GetClamp_Status() != 0)
                        {
                            if (sw.ElapsedMilliseconds > timeout_error)
                            {
                                return false;
                            }
                        }
                        return true;
                    }
                    return true;
                }
                else
                    return false;
            }
            catch (Exception ex)
            {
                return false;
            }
        }
        /// <summary>
        /// 夹紧速度
        /// </summary>
        /// <param name="vel">单位 mm/s，取值范围：1~200</param>
        /// <returns></returns>
        public bool Clamp_Vel(float vel)
        {
            try
            {
                string cmdStr = "011000040002 04" + FloatToByteString(vel);
                var src_crc = GetCRC16(cmdStr);
                var str = writeByte(src_crc);
                return str.IndexOf("011000040002") == 0;
            }
            catch (Exception ex)
            {
                return false;
            }
        }
        /// <summary>
        /// 夹紧电流
        /// </summary>
        /// <param name="vel">单位 A，取值范围：0.1~0.5</param>
        /// <returns></returns>
        public bool Clamp_A(float a)
        {
            try
            {
                string cmdStr = "0110 0006 0002 04" + FloatToByteString(a);
                var src_crc = GetCRC16(cmdStr);
                var str = writeByte(src_crc);
                return str.IndexOf("011000060002") == 0;
            }
            catch (Exception ex)
            {
                return false;
            }
        }
        /// <summary>
        /// 旋转角度
        /// </summary>
        /// <param name="angle">单位 度，取值范围：-3600000~3600000</param>
        /// <returns></returns>
        public bool Rotate_AngleAbs(float angle)
        {
            try
            {
                string cmdStr = "0110 000A 0002 04" + FloatToByteString(angle);
                var src_crc = GetCRC16(cmdStr);
                var str = writeByte(src_crc).ToUpper();
                if (str.IndexOf("0110000A0002") == 0)
                {
                    if (Isblock)//是否阻塞等待到位
                    {
                        Stopwatch sw = new Stopwatch();
                        sw.Restart();
                        while (GetRotate_Status() != 0)
                        {
                            if (sw.ElapsedMilliseconds > timeout_error)
                            {
                                return false;
                            }
                        }
                    }
                    return true;
                }
                else
                    return false;
            }
            catch (Exception ex)
            {
                return false;
            }
        }
        /// <summary>
        /// 旋转速度
        /// </summary>
        /// <param name="vel">单位 度/s，取值范围：1~1440</param>
        /// <returns></returns>
        public bool Rotate_Vel(float vel)
        {
            try
            {
                string cmdStr = "0110 000E 0002 04" + FloatToByteString(vel);
                var src_crc = GetCRC16(cmdStr);
                var str = writeByte(src_crc).ToUpper();
                return str.IndexOf("0110000E0002") == 0;
            }
            catch (Exception ex)
            {
                return false;
            }
        }
        /// <summary>
        /// 旋转电流
        /// </summary>
        /// <param name="a">单位 A，取值范围：0.2~1</param>
        /// <returns></returns>
        public bool Rotate_A(float a)
        {
            try
            {
                string cmdStr = "011000100002 04" + FloatToByteString(a);
                var src_crc = GetCRC16(cmdStr);
                var str = writeByte(src_crc).ToUpper();
                return str.IndexOf("011000100002") == 0;
            }
            catch (Exception ex)
            {
                return false;
            }
        }
        /// <summary>
        /// 相对旋转角度
        /// </summary>
        /// <param name="a">单位 度，取值范围：-3600000~3600000</param>
        /// <returns></returns>
        public bool Rotate_Angle(float angle)
        {
            try
            {
                string cmdStr = "0110 0014 0002 04" + FloatToByteString(angle);
                var src_crc = GetCRC16(cmdStr);
                //var curAngle = GetRotate_Angle();
                var str = writeByte(src_crc).ToUpper();
                if (str.IndexOf("011000140002") == 0)
                {
                    if (Isblock)//是否阻塞等待到位
                    {
                        Stopwatch sw = new Stopwatch();
                        sw.Restart();
                        while (GetRotate_Status() != 0)
                        {
                            if (sw.ElapsedMilliseconds > timeout_error)
                            {
                                return false;
                            }
                        }
                        return true;
                    }
                    return true;
                }
                else
                    return false;
                   
            }
            catch (Exception ex)
            {
                return false;
            }
        }
        /// <summary>
        /// 电机使能
        /// </summary>
        /// <param name="val">写0关闭电机输出，写1使能电机</param>
        /// <returns></returns>
        public bool Motor_Enable(int val)
        {
            try
            {
                string cmdStr = "01060016" + val.ToString("X").PadLeft(4, '0');
                var src_crc = GetCRC16(cmdStr);
                var str = writeByte(src_crc).ToUpper();
                return str.IndexOf("01060016") == 0;
            }
            catch (Exception ex)
            {
                return false;
            }
        }
        /// <summary>
        /// 夹持点位控制点
        /// </summary>
        /// <param name="val">夹持点位位置模式控制参数，选择通信触发的点位，取值范围：0~7。</param>
        /// <returns></returns>
        public bool Cample_IOSelect(int val)
        {
            try
            {
                string cmdStr = "01060017" + val.ToString("X").PadLeft(4, '0');
                var src_crc = GetCRC16(cmdStr);
                var str = writeByte(src_crc).ToUpper();
                return str.IndexOf("01060017") == 0;
            }
            catch (Exception ex)
            {
                return false;
            }
        }
        /// <summary>
        /// 夹持点位控制点
        /// </summary>
        /// <param name="val">旋转点位位置模式控制参数，选择通信触发的点位，取值范围：0~7。</param>
        /// <returns></returns>
        public bool Rotate_IOSelect(int val)
        {
            try
            {
                string cmdStr = "01060018" + val.ToString("X").PadLeft(4, '0');
                var src_crc = GetCRC16(cmdStr);
                var str = writeByte(src_crc).ToUpper();
                return str.IndexOf("01060018") == 0;
            }
            catch (Exception ex)
            {
                return false;
            }
        }
        #endregion
        #region 状态
        /// <summary>
        /// 获取当前初始化状态
        /// </summary>
        /// <returns> -1读取出错 0：未初始化，5：初始化完成，其它：初始化中</returns>
        public int GetInital_Status()
        {
            try
            {
                if (!serailPortJudge())
                    return -1;
                var str = "01 03 0040 0001";
                var str_status = send_intStatus(str);
                return Convert.ToInt32(str_status, 16);
            }
            catch (Exception ex)
            {
             
                return -1;
            }
        }
        /// <summary>
        /// 获取当前夹紧状态
        /// </summary>
        /// <returns>-1读取出错 0到位  1运动中 2夹持  3掉落</returns>
        public int GetClamp_Status()
        {
            try
            {
                if (!serailPortJudge())
                    return -1;
                var str = "01 03 0041 0001";
                var str_status = send_intStatus(str);
                return Convert.ToInt32(str_status, 16);
            }
            catch (Exception ex)
            {

                return -1;
            }
        }
        /// <summary>
        /// 获取当前夹持位置
        /// </summary>
        /// <returns> -1读取出错 单位 mm</returns>
        public float GetClamp_Pos()
        {
            try
            {
                if (!serailPortJudge())
                    return -1;
                var str = "01 03 0042 0001";
                var str_status = send_floatStatus(str);
                return UshortArrayToFloat(str_status);
            }
            catch (Exception ex)
            {
               
                return -1;
            }
        }
        /// <summary>
        /// 获取当前夹持速度
        /// </summary>
        /// <returns>-1 表示读不到  单位 mm/s</returns>
        public float GetClamp_vel()
        {
            try
            {
                if (!serailPortJudge())
                    return -1;
                var str = "01 03 0044 0001";
                var str_status = send_floatStatus(str);
                return UshortArrayToFloat(str_status);
            }
            catch (Exception ex)
            {
               
                return -1;
            }
        }
        /// <summary>
        /// 获取当前夹持电流
        /// </summary>
        /// <returns>-1 表示读不到 单位 A</returns>
        public float GetClamp_A()
        {
            try
            {
                if (!serailPortJudge())
                    return -1;
                var str = "01 03 0046 0001";
                var str_status = send_floatStatus(str);
                return UshortArrayToFloat(str_status);
            }
            catch (Exception ex)
            {
                
                return -1;
            }
        }
        /// <summary>
        /// 获取旋转状态
        /// </summary>
        /// <returns>-1 读取出错 0到位 1旋转中 2堵转  </returns>
        public int GetRotate_Status()
        {
            try
            {
                if (!serailPortJudge())
                    return -1;
                var str = "01 03 0048 0001";
                var str_status = send_intStatus(str);
                return Convert.ToInt32(str_status, 16);
            }
            catch (Exception ex)
            {
               
                return -1;
            }
        }
        /// <summary>
        /// 获取旋转角度
        /// </summary>
        /// <returns>-1有可能属于读取出错  单位 度</returns>
        public float GetRotate_Angle()
        {
            try
            {
                if (!serailPortJudge())
                    return -1;
                var str = "01 03 004A 0001";
                var str_status = send_floatStatus(str);
                return UshortArrayToFloat(str_status);
            }
            catch (Exception ex)
            {
                return -1;
            }
        }
        /// <summary>
        /// 获取旋转速度
        /// </summary>
        /// <returns>>-1 读取出错 单位 度/s</returns>
        public float GetRotate_vel()
        {
            try
            {
                if (!serailPortJudge())
                    return -1;
                var str = "01 03 004C 0001";
                var str_status = send_floatStatus(str);
                return UshortArrayToFloat(str_status);
            }
            catch (Exception ex)
            {
                return -1;
            }
        }
        /// <summary>
        /// 获取旋转电流
        /// </summary>
        /// <returns>-1 可能是读取错误  单位 A</returns>
        public float GetRotate_A()
        {
            try
            {
                if (!serailPortJudge())
                    return -1;
                var str = "01 03 004E 0001";
                var str_status = send_floatStatus(str);
                return UshortArrayToFloat(str_status);
            }
            catch (Exception ex)
            {
                return -1;
            }
        }
        #endregion
        #region 参数设置
        /// <summary>
        /// 设置ID
        /// </summary>
        /// <param name="val">取值范围 [1--247] ，保存参数后重启有效</param>
        /// <returns></returns>
        public bool SetParma_ID(int val)
        {
            try
            {
                if (!serailPortJudge())
                    return false;
                var str = "01 06 00 80"+ val.ToString("X").PadLeft(4, '0');
                var str_status = send_intStatus(str);
                return val==Convert.ToInt32(str_status, 16);
            }
            catch (Exception ex)
            {
                
                return false;
            }
        }
        /// <summary>
        /// 设置波特率
        /// </summary>
        /// <param name="val">0--8      :baud9600，baud19200，baud38400，baud57600，baud115200，baud153600，baud256000，baud460800，baud921600</param>
        /// <returns></returns>
        public bool SetParma_baud(int val)
        {
            try
            {
                if (!serailPortJudge())
                    return false;
                var str = "01 06 00 81" + val.ToString("X").PadLeft(4, '0');
                var str_status = send_intStatus(str);
                return val == Convert.ToInt32(str_status, 16);
            }
            catch (Exception ex)
            {
                return false;
            }
        }
        /// <summary>
        /// 设置初始化方向
        /// </summary>
        /// <param name="val">0张开校准  1闭合校准 </param>
        /// <returns></returns>
        public bool SetParma_intalOrder(int val)
        {
            try
            {
                if (!serailPortJudge())
                    return false;
                var str = "01 06 00 82" + val.ToString("X").PadLeft(4, '0');
                var str_status = send_intStatus(str);
                return val == Convert.ToInt32(str_status, 16);
            }
            catch (Exception ex)
            {

                return false;
            }
        }
        /// <summary>
        /// 设置自动初始化
        /// </summary>
        /// <param name="val">0上电自动校准  1上电手动校准 保存参数后重启有效</param>
        /// <returns></returns>
        public bool SetParma_intalAutoCalibra(int val)
        {
            try
            {
                if (!serailPortJudge())
                    return false;
                var str = "01 06 00 83" + val.ToString("X").PadLeft(4, '0');
                var str_status = send_intStatus(str);
                return val == Convert.ToInt32(str_status, 16);
            }
            catch (Exception ex)
            {

                return false;
            }
        }
        /// <summary>
        /// 保存参数设置 写1保存参数，    注意：不要在实时控制时使用此命令，仅在实际使用前配置参数使用
        /// </summary>
        /// <returns></returns>
        public bool SetParma_Save()
        {
            int val = 1;
            try
            {
                if (!serailPortJudge())
                    return false;
                var str = "01 06 00 84" + val.ToString("X").PadLeft(4,'0');
                var str_status = send_intStatus(str);
                return val == Convert.ToInt32(str_status, 16);
            }
            catch (Exception ex)
            {

                return false;
            }
        }
        /// <summary>
        /// 恢复默认参数
        /// </summary>
        /// <returns></returns>
        public bool SetParma_Default()
        {
            int val = 1;
            try
            {
                if (!serailPortJudge())
                    return false;
                var str = "01 06 00 85" + val.ToString("X").PadLeft(4, '0');
                var str_status = send_intStatus(str);
                return val == Convert.ToInt32(str_status, 16);
            }
            catch (Exception ex)
            {

                return false;
            }
        }
        /// <summary>
        /// 设置大小端
        /// </summary>
        /// <param name="val">1小端 0大端 保存参数后重启有效</param>
        /// <returns></returns>
        public bool SetParma_BigPort(int val)
        {
           
            try
            {
                if (!serailPortJudge())
                    return false;
                var str = "01 06 00 89" + val.ToString("X").PadLeft(4, '0');
                var str_status = send_intStatus(str);
                return val == Convert.ToInt32(str_status, 16);
            }
            catch (Exception ex)
            {

                return false;
            }
        }
        /// <summary>
        /// 复位多圈转动值  复位旋转角度为±360°以内，当前角度大于0°复位到0-360，小于0°复位到-360-0
        /// </summary>
        /// <returns></returns>
        public bool SetParma_ResetRotate()
        {
            int val = 1;
            try
            {
                if (!serailPortJudge())
                    return false;
                var str = "01 06 00 8F" + val.ToString("X").PadLeft(4, '0');
                var str_status = send_intStatus(str);
                return val == Convert.ToInt32(str_status, 16);
            }
            catch (Exception ex)
            {

                return false;
            }
        }
        /// <summary>
        /// 关闭/打开IO模式
        /// </summary>
        /// <param name="val">0关闭 1打开</param>
        /// <returns></returns>
        public bool SetParma_IOModel(int val)
        {
            try
            {
                if (!serailPortJudge())
                    return false;
                var str = "01 06 00 90" + val.ToString("X").PadLeft(4, '0');
                var str_status = send_intStatus(str);
                return val == Convert.ToInt32(str_status, 16);
            }
            catch (Exception ex)
            {

                return false;
            }
        }
        /// <summary>
        /// 设置停转堵装使能
        /// </summary>
        /// <param name="val">0：不使能，1：使能，默认开放此功能</param>
        /// <returns></returns>
        public bool SetParma_RotateBlockEnable(int val)
        {
            try
            {
                if (!serailPortJudge())
                    return false;
                var str = "01 06 00 9E" + val.ToString("X").PadLeft(4, '0');
                var str_status = send_intStatus(str);
                return val == Convert.ToInt32(str_status, 16);
            }
            catch (Exception ex)
            {

                return false;
            }
        }
        /// <summary>
        /// 设置停转堵装灵敏度
        /// </summary>
        /// <param name="val">0-100，数字越大灵敏度越好</param>
        /// <returns></returns>
        public bool SetParma_RotateBlockRatio(int val)
        {
            try
            {
                if (!serailPortJudge())
                    return false;
                var str = "01 06 00 9F" + val.ToString("X").PadLeft(4, '0');
                var str_status = send_intStatus(str);
                return val == Convert.ToInt32(str_status, 16);
            }
            catch (Exception ex)
            {

                return false;
            }
        }
        /// <summary>
        /// 输入NPN或者PNP设置
        /// </summary>
        /// <param name="val">0为NPN，1为PNP</param>
        /// <returns></returns>
        public bool SetParma_NPNinput(int val)
        {
            try
            {
                if (!serailPortJudge())
                    return false;
                var str = "01 06 00 A0" + val.ToString("X").PadLeft(4, '0');
                var str_status = send_intStatus(str);
                return val == Convert.ToInt32(str_status, 16);
            }
            catch (Exception ex)
            {

                return false;
            }
        }
        /// <summary>
        /// 输出NPN或者PNP设置
        /// </summary>
        /// <param name="val">0为NPN，1为PNP</param>
        /// <returns></returns>
        public bool SetParma_NPNOutput(int val)
        {
            try
            {
                if (!serailPortJudge())
                    return false;
                var str = "01 06 00 A1" + val.ToString("X").PadLeft(4, '0');
                var str_status = send_intStatus(str);
                return val == Convert.ToInt32(str_status, 16);
            }
            catch (Exception ex)
            {

                return false;
            }
        }

        #endregion
        #region 辅助方法
        /// <summary>
        /// 发送后立马接收
        /// </summary>
        /// <param name="str"></param>
        /// <returns></returns>
        private string send_intStatus(string str)
        {
            var Byte_send = GetCRC16(str);
            string str_status = writeByte(Byte_send);
            str_status = str_status.Substring(str_status.Length - 8, 4);
            return str_status;
        }
        private string send_floatStatus(string str)
        {
            var Byte_send = GetCRC16(str);
            var temp = "";
            foreach (var b in Byte_send)
            {
                temp += Convert.ToString(b, 16).PadLeft(2, '0');
            }
            string str_status = writeByte(Byte_send);
            str_status = str_status.Substring(str_status.Length - 12, 8);
            return str_status;
        }
        private string writeByte(byte[] Byte_send)
        {
            string str_status = "";
            are_clamp.WaitOne();
            try
            {
                m_sp.DiscardInBuffer();
                m_sp.Write(Byte_send, 0, Byte_send.Length);
                Application.DoEvents();
                Stopwatch sw = new Stopwatch();
                sw.Restart();
                int length = m_sp.BytesToRead;
                while (length < Byte_send.Length)
                {
                    Application.DoEvents();
                    length = m_sp.BytesToRead;
                    if (sw.ElapsedMilliseconds > 100)
                        break;
                }
                byte[] buffer = new byte[length];
                m_sp.Read(buffer, 0, length);
                are_clamp.Set();
                foreach (var b in buffer)
                {
                    str_status += Convert.ToString(b, 16).PadLeft(2, '0');
                }
            }
            catch (Exception ex)
            {
                are_clamp.Set();
                throw ex;
            }
            return str_status;
        }
        /// <summary>
        /// short数组转换成浮点数
        /// </summary>
        /// <param name="input">short数组</param>
        /// <returns>浮点数</returns>
        public static float UshortArrayToFloat(string input)
        {
            float ret = -1.0f;
            try
            {
                if (input == null || input.Length < 1)
                {
                    return ret;
                }
                //Convert Float to short 
                byte[] temp = new byte[4];
                temp[0] = Convert.ToByte(input.Substring(0,2), 16);
                temp[1] = Convert.ToByte(input.Substring(2,2), 16);
                temp[2] = Convert.ToByte(input.Substring(4, 2), 16);
                temp[3] = Convert.ToByte(input.Substring(6, 2), 16);
                ret = BitConverter.ToSingle(temp,0);
          
                return ret;
            }
            catch (System.Exception ex)
            {
                throw ex;
            }

        }
        /// <summary>
        /// 在字节字符串后追加校验码
        /// </summary>
        /// <param name="sdata"></param>
        /// <param name="poly"></param>
        /// <param name="crcInit"></param>
        /// <returns></returns>
        public byte[] GetCRC16(string sdata, ushort poly = 0xA001, ushort crcInit = 0xFFFF)
        {
            if (sdata.Contains(" "))
                sdata = sdata.Replace(" ", "");
            int slength = sdata.Length / 2;
            List<byte> bList = new List<byte>();
            for (int i = 0; i < slength; i++)
            {
                var s = "0x" + sdata.Substring(i * 2, 2);
                bList.Add(Convert.ToByte(s, 16));
            }
            byte[] value = bList.ToArray();
            if (value == null || !value.Any())
                throw new ArgumentException("生成CRC16的入参有误");
            //运算
            ushort crc = crcInit;
            for (int i = 0; i < value.Length; i++)
            {
                crc = (ushort)(crc ^ (value[i]));
                for (int j = 0; j < 8; j++)
                {
                    crc = (crc & 1) != 0 ? (ushort)((crc >> 1) ^ poly) : (ushort)(crc >> 1);
                }
            }
            byte hi = (byte)((crc & 0xFF00) >> 8);  //高位置
            byte lo = (byte)(crc & 0x00FF);         //低位置

            byte[] buffer = new byte[value.Length + 2];
            value.CopyTo(buffer, 0);
            buffer[buffer.Length - 1] = hi;
            buffer[buffer.Length - 2] = lo;
            return buffer;
        }
        /// <summary>
        /// 
        /// </summary>
        /// <param name="input"></param>
        /// <returns></returns>
        private string FloatToByteString(float input)
        {
            ushort[] uintData = new ushort[2];
            try
            {
                float[] floatData = new float[1] { input };
                Buffer.BlockCopy(floatData, 0, uintData, 0, 4);
                string str = "";
                for (int i = uintData.Length-1; i >=0 ; i--)
                    str += uintData[i].ToString("X").PadLeft(4, '0');
                return str;
            }
            catch (System.Exception ex)
            {

                throw ex;
            }

        }
        #endregion

    }
}
